#include "SerialProtocol.h"
#include "usart.h"

void SerialProtocol_PutByte(SerialProtocol_HandleTypedef *handle, uint8_t data) {
    switch (handle->state) {
        case STATE_IDLE: {
            if (data == 0x80) {
                handle->state = STATE_ADDR;
            }
            break;
        }
        case STATE_ADDR: {
            handle->packetAddress = data;
            handle->state = STATE_LENGTH;
            break;
        }
        case STATE_LENGTH: {
            handle->packetLength = data;
            handle->bufferIndex = 0;
            if (data != 0)handle->state = STATE_DATA;
            else handle->state = STATE_STOP;
            break;
        }
        case STATE_DATA: {
            if (handle->bufferIndex < handle->packetLength - 1) {
                handle->buffer[handle->bufferIndex] = data;
                handle->bufferIndex++;
            } else {
                handle->buffer[handle->bufferIndex] = data;
                handle->state = STATE_STOP;
            }
            break;
        }
        case STATE_STOP: {
            if (data == 0xff) {
                handle->callbackReady = true;
                handle->state = STATE_WAIT_CB;
            }
        }
        case STATE_WAIT_CB: {
            break;
        }
        default:
            break;
    }
}

void SerialProtocol_InitHandle(SerialProtocol_HandleTypedef *handle, uint8_t bufferSize) {
    handle->buffer = (uint8_t *)malloc(bufferSize);
    handle->bufferIndex = 0;
    handle->callbackRegistered = false;
    handle->callbackFunc = NULL;
    handle->callbackReady = false;
}

void SerialProtocol_RegisterCallback(SerialProtocol_HandleTypedef *handle, SerialProtocol_CallbackTypedef callback) {
    handle->callbackFunc = callback;
    handle->callbackRegistered = true;
}


int SerialProtocol_RequestCallback(SerialProtocol_HandleTypedef *handle) {
    if (handle->callbackReady && handle->callbackRegistered) {
        handle->callbackFunc(handle->packetAddress, handle->packetLength, handle->buffer);
        handle->callbackReady = false;
        handle->state = STATE_IDLE;
        return 0;
    } else {
        return -1;
    }
}

void port_SerialProtocolWriteSerial(uint8_t data) {
    HAL_UART_Transmit(&huart5, &data, 1,HAL_MAX_DELAY);
}

void SerialProtocol_Reply(SerialProtocol_HandleTypedef *handle, uint8_t *data, uint8_t length) {
    uint8_t i;
    port_SerialProtocolWriteSerial(0x90);
    port_SerialProtocolWriteSerial(handle->packetAddress);
    port_SerialProtocolWriteSerial(length);
    for (i = 0; i < length; i++) {
        port_SerialProtocolWriteSerial(data[i]);
    }
    port_SerialProtocolWriteSerial(0xff);
}

void SerialProtocol_ResetStateMachine(SerialProtocol_HandleTypedef *handle) {
    handle->state = STATE_IDLE;
}
